import numpy as np
import matplotlib.pyplot as plt
from math import pow as power

from robot_pose import robot_pose_after_tracking,robot_pose_before_executing,robot_pose_after_planning
from path import filtered_path,raw_path,running_path

# robot_pos_update = None

def scatter_data_in_plt(data,c = None,m=None):
    if data:
        arr = np.array(data)
        plt.scatter(arr[:,0],arr[:,1],c = c,marker=m)
def plot_data_in_plt(data,c = None,m=None):
    if data:
        arr = np.array(data)
        plt.plot(arr[:,0],arr[:,1],c=c,marker=m)
def vis_data(input_data,scatter_data=None,scatter_data2=None,scatter_data3=None):
    plt.xlim(-2, 2)
    plt.ylim(-2,2)
    for line in input_data:
        scatter_data_in_plt(line)
        plot_data_in_plt(line)
    scatter_data_in_plt(scatter_data,c="black")
    scatter_data_in_plt(scatter_data2,c="blue",m="1")
    scatter_data_in_plt(scatter_data3,c = "red",m="2")
    plt.show()

def vis_2_path(input_data,input_data2,scatter_data=None,scatter_data2=None,scatter_data3=None):

    for line in input_data:
        scatter_data_in_plt(line)
        plot_data_in_plt(line)
    for line in input_data2:
        scatter_data_in_plt(line)
        plot_data_in_plt(line)
    scatter_data_in_plt(scatter_data,c="black")
    scatter_data_in_plt(scatter_data2,c="blue")
    scatter_data_in_plt(scatter_data3,c="red")
    plt.show()

if __name__ == '__main__':
    vis_data(filtered_path,robot_pose_after_tracking,robot_pose_before_executing,robot_pose_after_planning)
    vis_2_path(running_path,filtered_path,robot_pose_after_tracking,robot_pose_before_executing)